#include "rcl_interfaces/srv/set_parameters.hpp"
#include "rclcpp/rclcpp.hpp"

class Client_node : public rclcpp::Node
{
  private:
	int cycle = 0;
	int going = 0;
	rclcpp::Client<rcl_interfaces::srv::SetParameters>::SharedPtr setclient;
	rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr getclient;
	std::shared_ptr<rclcpp::TimerBase> timer;
	int param_int_a;
	double param_double_a;
	std::string param_string_a;

	// called after parameters were set
	void setreq_callback(
		std::shared_future<rcl_interfaces::srv::SetParameters::Response::SharedPtr>)
	{
		RCLCPP_INFO(this->get_logger(), "parameters successfully set");
		cycle++;
		going = 0;
	}

	// called after we got our parameters
	void getreq_callback(
		std::shared_future<rcl_interfaces::srv::GetParameters::Response::SharedPtr> resf)
	{
		auto res = resf.get();
		auto setreq = std::make_shared<rcl_interfaces::srv::SetParameters_Request>();
		setreq->parameters.resize(1);
		switch (res->values[0].type)
		{
			case rclcpp::ParameterType::PARAMETER_INTEGER:
				param_int_a = res->values[0].integer_value;
				setreq->parameters[0].name = "param_int_a";
				setreq->parameters[0].value.type = rclcpp::ParameterType::PARAMETER_INTEGER;
				setreq->parameters[0].value.integer_value = param_int_a + 1;
				break;
			case rclcpp::ParameterType::PARAMETER_DOUBLE:
				param_double_a = res->values[0].double_value;
				setreq->parameters[0].name = "param_double_a";
				setreq->parameters[0].value.type = rclcpp::ParameterType::PARAMETER_DOUBLE;
				setreq->parameters[0].value.double_value = param_double_a + 0.1;
				break;
			case rclcpp::ParameterType::PARAMETER_STRING:
				param_string_a = res->values[0].string_value;
				setreq->parameters[0].name = "param_string_a";
				setreq->parameters[0].value.type = rclcpp::ParameterType::PARAMETER_STRING;
				setreq->parameters[0].value.string_value =
					"int: " + std::to_string(param_int_a) +
					", double: " + std::to_string(param_double_a);
				break;
			default:
				return;
		}

		RCLCPP_INFO(this->get_logger(), "got parameter type: %d", res->values[0].type);
		setclient->async_send_request(
			setreq, std::bind(&Client_node::setreq_callback, this, std::placeholders::_1));
		RCLCPP_INFO(this->get_logger(), "sent set_parameters request");
	}

	void timer_callback(void)
	{
		auto getreq = std::make_shared<rcl_interfaces::srv::GetParameters_Request>();
		getreq->names.resize(1);
		if (going)
			return;
		if (cycle == 0)
		{
			getreq->names[0] = "param_int_a";
			getclient->async_send_request(
				getreq, std::bind(&Client_node::getreq_callback, this, std::placeholders::_1));
			RCLCPP_INFO(this->get_logger(), "sent get_parameters request(int)");
			going = 1;
		}
		else if (cycle == 1)
		{
			getreq->names[0] = "param_double_a";
			getclient->async_send_request(
				getreq, std::bind(&Client_node::getreq_callback, this, std::placeholders::_1));
			RCLCPP_INFO(this->get_logger(), "sent get_parameters request(double)");
			going = 1;
		}
		else if (cycle == 2)
		{
			getreq->names[0] = "param_string_a";
			getclient->async_send_request(
				getreq, std::bind(&Client_node::getreq_callback, this, std::placeholders::_1));
			RCLCPP_INFO(this->get_logger(), "sent get_parameters request(string)");
			going = 1;
		}
		else if (cycle >= 3)
		{
			cycle = 0;
		}
	}

  public:
	Client_node(std::string name) : Node(name)
	{
		RCLCPP_INFO(this->get_logger(), "node started");
		setclient = this->create_client<rcl_interfaces::srv::SetParameters>(
			"param_node/set_parameters");
		getclient = this->create_client<rcl_interfaces::srv::GetParameters>(
			"param_node/get_parameters");
		RCLCPP_INFO(this->get_logger(), "created clients");
		timer = this->create_wall_timer(std::chrono::milliseconds(100),
										std::bind(&Client_node::timer_callback, this));
		RCLCPP_INFO(this->get_logger(), "created timer");
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<Client_node>("client_node");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
